/*
 *Copyright (c) 2024 Black Sesame Technologies
 *
 *Licensed under the Apache License, Version 2.0 (the "License");
 *you may not use this file except in compliance with the License.
 *You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 *Unless required by applicable law or agreed to in writing, software
 *distributed under the License is distributed on an "AS IS" BASIS,
 *WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *See the License for the specific language governing permissions and
 *limitations under the License.
*/

#include "FlexCAN_Ip.h"

#ifndef container_of
#define offsetof(TYPE, MEMBER) ((unsigned int) &((TYPE *)0)->MEMBER)

#define container_of(ptr, type, member) ({			\
	const typeof( ((type *)0)->member ) *__mptr = (ptr);	\
	(type *)( (char *)__mptr - offsetof(type,member) );})
#endif

#ifndef ALIGN_UP
#define ALIGN_UP(_x, _align_to) (((_x) + ((_align_to) - 1)) & (~((_align_to) - 1)))
#endif

#define TX_MB_MEMORY_QUEUE_POOL_SIZE 20*1024

struct msgn_node {
    void *next;
    unsigned int ref;
    unsigned int data[0];
};

struct msgn_ctrl {
    void *buffer;
    unsigned int buf_size;
    unsigned int msg_node_size;
    struct msgn_node *first;
};

static uint8 __attribute__((aligned(16))) can_msg_buffer[TX_MB_MEMORY_QUEUE_POOL_SIZE];
static void *can_msg_cb;

static void* msgn_init(void *buffer, unsigned int buf_size, unsigned int msg_node_size)
{
    struct msgn_ctrl *cb;
    struct msgn_node *node;
    struct msgn_node *prev;
    unsigned char *pos;
    unsigned char *last;

    if (buffer == NULL || buf_size < sizeof(struct msgn_ctrl) || buf_size < msg_node_size) {
        return NULL;
    }

    cb = (struct msgn_ctrl*)buffer;
    cb->buffer = buffer;
    cb->buf_size = buf_size;
    cb->msg_node_size = ALIGN_UP(msg_node_size, 4) + sizeof(struct msgn_node);
    cb->first = (struct msgn_node*)(cb + 1);

    prev = cb->first;
    pos = (unsigned char*)(cb->first) + cb->msg_node_size;
    last = (unsigned char*)buffer + buf_size - cb->msg_node_size;
    while (pos <= last)
    {
        node = (struct msgn_node*)(pos);
        prev->next = node;
        prev = node;
        pos += cb->msg_node_size;
    }

    prev->next = NULL;

    return cb;
}

static inline struct msgn_node* to_node(void *msg)
{
    return msg ? container_of(msg, struct msgn_node, data) : NULL;
}

static inline unsigned int msgn_in_range(struct msgn_ctrl *cb, void *msg)
{
    if ((unsigned char*)msg < (unsigned char*)cb->buffer
        || (unsigned char*)msg >= ((unsigned char*)cb->buffer + cb->buf_size)) {
        return 0;
    }
    return 1;
}

static void* msgn_alloc(void *msg_cb)
{
    struct msgn_ctrl *cb = (struct msgn_ctrl*)msg_cb;
    struct msgn_node *node;

    if (!cb->first) {
        return NULL;
    }

    node = cb->first;
    cb->first = node->next;
    node->next = NULL;
    node->ref = 1;

    return node->data;
}

static void* msgn_shallow(void *msg)
{
    struct msgn_node *node = to_node(msg);

    if (node) {
        ++node->ref;
    }
    return msg;
}

static void msgn_free(void *msg_cb, void *msg)
{
    struct msgn_ctrl *cb = (struct msgn_ctrl*)msg_cb;
    struct msgn_node *node = to_node(msg);

    if (!msg) {
        return;
    }

    if (!msgn_in_range(msg_cb, msg)) {
        return;
    }

    if (node->ref == 0) {
        hprintf(ERROR, "error, ref is 0 in msgn_free!\r\n");
        return;
    }

    --node->ref;
    if (node->ref) {
        return;
    }

    node->next = cb->first;
    cb->first = node;
}

void can_msg_init(void)
{
    can_msg_cb = msgn_init(can_msg_buffer, sizeof(can_msg_buffer), sizeof(Flexcan_Ip_MsgBuffType));
}

void* can_msg_alloc(void)
{
    return msgn_alloc(can_msg_cb);
}

void can_msg_free(void *msg)
{
    return msgn_free(can_msg_cb, msg);
}

void* can_msg_shallow(void *msg)
{
    return msgn_shallow(msg);
}
